
// poseEstimate.h
#ifndef POSE_ESTIMATE_H
#define POSE_ESTIMATE_H

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <iomanip>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <opengv/relative_pose/methods.hpp>
#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
#include <cstdlib>

class PoseEstimator {
public:
    PoseEstimator(); // 构造函数

void find_feature_matches(const cv::Mat &img_1, const cv::Mat &img_2, std::vector<cv::KeyPoint> &keypoints_1, std::vector<cv::KeyPoint> &keypoints_2, std::vector<cv::DMatch> &matches, int nfeatures);
void pose_CV_essential(const std::vector<cv::KeyPoint> &keypoints_1, const std::vector<cv::KeyPoint> &keypoints_2, const std::vector<cv::DMatch> &matches, cv::Mat &R, cv::Mat &t, int method);
void pose_CV_fundamental(const std::vector<cv::KeyPoint> &keypoints_1, const std::vector<cv::KeyPoint> &keypoints_2, const std::vector<cv::DMatch> &matches, cv::Mat &R, cv::Mat &t, int method);
std::pair<Eigen::Matrix3d, Eigen::Vector3d> pose_GV_eightpt(const std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2, std::vector<cv::DMatch>& matches);
std::pair<Eigen::Matrix3d, Eigen::Vector3d> pose_GV_eigensolver(const std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2, std::vector<cv::DMatch>& matches);
void convertCvMatToEigen(const cv::Mat& cvMat, Eigen::Matrix3d& eigenMat);
void convertEigenToCvMat(const Eigen::Vector3d& eigenVec, cv::Mat& cvMat);
std::vector<std::string> split(const std::string &s, char delimiter);
void savePose(std::ifstream &timefile, std::ofstream &outfile, const cv::Mat &t_cv, const Eigen::Quaterniond &q);
void getRMSE(const std::string& groundTruthPath, const std::string& estimatePath);
};

#endif